#pragma once
//读写csv文件所需
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <iostream>
#include <string>
#include <vector>
#include <fstream>
#include <sstream>
#include <random>
#include<algorithm>

#include "auto_msgs/planning_point.h"
#include "auto_msgs/planning.h"
#include <vector>
#include "nanoflann.hpp"
#include <ctime>
#include <cstdlib>
#include <iostream>
using namespace nanoflann;
struct PlanningPath
{
    public:

        void write_path_to_csv(const std::string filename,int cloum_dim = 7)
        {
            std::ofstream outFile;
            outFile.open(filename,std::ios::ate|std::ios::out );
            // 5000*128
            int row_dim = path_.path.size();
            for(int i=0;i<row_dim;i++){
                outFile<< path_.path[i].x<<',';
                outFile<< path_.path[i].y<<',';
                outFile<< path_.path[i].theta<<',';
                outFile<< path_.path[i].kappa<<',';
                outFile<< path_.path[i].v<<',';
                outFile<< path_.path[i].s<<',';
                outFile<< path_.path[i].a<<',';
                if(i<row_dim){
                    outFile<<std::endl;
                }                
            }

        }
        void read_csv_to_path(const std::string filename,bool squeze=true)
        {

            std::vector<std::string> row_element;
            std::string line;
            auto_msgs::planning_point temp_point;
            std::ifstream in(filename,std::ios::in);
            path_.path.clear();     
            if (in.fail())  { std::cout << "File not found" <<std::endl; return ; }
            std::cout<<"oh"<<std::endl ;
            while(getline(in, line)  && in.good() )
            {
                if(squeze==true)
                {
                    squeze=false;
                }
                else
                {                                                          
                    row_to_string_vector(row_element, line, ',');  //把line里的单元格数字字符提取出来，“,”为单元格分隔符 
                    temp_point.x =atof( row_element[0].c_str() );
                    temp_point.y =atof( row_element[1].c_str() );
                    temp_point.theta =atof( row_element[2].c_str() );
                    temp_point.kappa =atof( row_element[3].c_str() );
                    temp_point.v =atof( row_element[4].c_str() );
                    temp_point.s =atof( row_element[5].c_str() );
                    temp_point.a =atof( row_element[6].c_str() );
                    path_.path.push_back(temp_point);
                }
            }
            in.close();
            return ;
        }
        void row_to_string_vector(std::vector<std::string> &record, const std::string& line, char delimiter)
        {
            int linepos=0;
            char c;
            int linemax=line.length();
            std::string curstring;
            record.clear();
            while(linepos<linemax)
            {
                c = line[linepos];
                if(isdigit(c)||c=='.'||c=='-'){
                    curstring+=c;
                }
                else if(c==delimiter&&curstring.size()){
                    record.push_back(curstring);
                    curstring="";
                }
                ++linepos;
            }
            if(curstring.size())
                record.push_back(curstring);
            return;
        }//end row_to_string_vector function

        auto_msgs::planning path_;

};

template <typename Derived>
struct PlanningPathAdaptor
{
	//typedef typename Derived::coord_t coord_t;

	const Derived &obj; //!< A const ref to the data set origin

	/// The constructor that sets the data set source
	PlanningPathAdaptor(const Derived &obj_) : obj(obj_) { }

	/// CRTP helper method
	inline const Derived& derived() const { return obj; }

	// Must return the number of data points
	inline size_t kdtree_get_point_count() const { return derived().path_.path.size(); }

	// Returns the dim'th component of the idx'th point in the class:
	// Since this is inlined and the "dim" argument is typically an immediate value, the
	//  "if/else's" are actually solved at compile time.
	inline double kdtree_get_pt(const size_t idx, const size_t dim) const
	{
		if (dim == 0) return derived().path_.path[idx].x;
		else if (dim == 1) return derived().path_.path[idx].y;
		else return derived().path_.path[idx].y;
	}

	// Optional bounding-box computation: return false to default to a standard bbox computation loop.
	//   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
	//   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
	template <class BBOX>
	bool kdtree_get_bbox(BBOX& /*bb*/) const { return false; }

}; // end of PlannigPathAdaptor
typedef PlanningPathAdaptor< PlanningPath > PlanningPath2KD;
typedef KDTreeSingleIndexAdaptor<
    L2_Simple_Adaptor<double, PlanningPath2KD > ,
    PlanningPath2KD,
    2 /* dim */
    > planning_kd_tree_t;

void dump_mem_usage()
{
	FILE* f=fopen("/proc/self/statm","rt");
	if (!f) return;
	char str[300];
	size_t n=fread(str,1,200,f);
	str[n]=0;
	printf("MEM: %s\n",str);
	fclose(f);
}
